#include "MeasurE616AlgBackSe1ToSe5Edge.h"

MeasurE616AlgBackSE1toSE5Edge::MeasurE616AlgBackSE1toSE5Edge(int i):MeasurE616AlgBase(i)
{

}

void MeasurE616AlgBackSE1toSE5Edge::operator()(cv::Mat &srcImg, int id,std::map<string, float> &measRes,
                                               int fixtureId)
{
    //LD部分边缘提取，返回点位   //SE1~SE5 back图  \paras616\back\SE.xml 前五行；
//    int LDmeasure(cv::Mat srcimg, const vector<int> paras, vector<cv::Point2f>& respts);
    std::vector<cv::Point2f> resptVec;
    for(auto i=22;i<27;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-22,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> contours;
        MeasurE616EdgeDetect obj;
        obj.LDmeasure(imgt,tparas,contours);
        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
        while (!MeasurE616AlgBase::flagVec[id])
        {
            (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
            resptVec.push_back(ptsOut);
        }
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    float fai14=0;
    for(int i=0;i<3;++i){
        fai14+=resptVec[i].y/3.0;
    }
    measRes.insert(std::make_pair(string("FAI14-SE1-Y-distance_pixel"), fai14));
    float fai14sey = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI14-SE1-Y-distance")], fai14);
    measRes.insert(std::make_pair(string("FAI14-SE1-Y-distance_real"), fai14sey));

    float fai7=0;
    for(int i=3;i<6;++i){
        fai7+=resptVec[i].x/3.0;
    }
    measRes.insert(std::make_pair(string("FAI7-SE2-X-distance_pixel"), fai7));
    float fai7sex = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI7-SE2-X-distance")], fai7);
    measRes.insert(std::make_pair(string("FAI7-SE2-X-distance_real"), fai7sex));

    float fai9=0;
    for(int i=6;i<8;++i){
        fai9+=resptVec[i].x/2.0;
    }
    measRes.insert(std::make_pair(string("FAI9-SE3-X-distance_pixel"), fai9));
    float fai9sex = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI9-SE3-X-distance")], fai9);
    measRes.insert(std::make_pair(string("FAI9-SE3-X-distance_real"), fai9sex));

    float fai5=0;
    for(int i=8;i<11;++i){
        fai5+=resptVec[i].y/3.0;
    }
    measRes.insert(std::make_pair(string("FAI5-SE4-Y-distance_pixel"), fai5));
    float fai5sey = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI5-SE4-Y-distance")], fai5);
    measRes.insert(std::make_pair(string("FAI5-SE4-Y-distance_real"), fai5sey));

    float fai11=0;
    for(int i=11;i<13;++i){
        fai11+=resptVec[i].x/2.0;
    }
    measRes.insert(std::make_pair(string("FAI11-SE5-X-distance_pixel"), fai11));
    float fai11sex = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI11-SE5-X-distance")], fai11);
    measRes.insert(std::make_pair(string("FAI11-SE5-X-distance_real"), fai11sex));
}
